#include "drive.h"
#include "core.h"

static uint8_t irq_cnt;


void drive_init(void) {
    uint8_t val;
/* =====================system init====================== */
    sys_init();
/* =====================timer init====================== */
    timer_init();
/* =====================cc init====================== */
    cc_init();
/* =====================ldo init====================== */
#if (CONFIG_IC_TYPE == CHIP_SS888X)
    ldo_init();
#endif
/* =====================adc init====================== */
    adc_init();
/* =====================pwm init====================== */
    pwm_init();
/* =====================cmp init====================== */
    cmp_init();
/* =====================power init====================== */
    power_init();
    /* As late as possible */
/* =====================uart init====================== */
    /* put it to last, it can override the MFP settings */
    uart_init();
/* =====================gpio init====================== */
    gpio_init();
/* =====================si2c init====================== */
    si2c_master_init();
/* =====================charging init====================== */
    charging_init();
/* =====================pfo init====================== */
    pfo_init();

#if (CONFIG_IC_TYPE == CHIP_SS888X)
/* =====================xdata init====================== */
    xdata_init();
/* =====================xdata init====================== */
    spi_init();
#endif
    /**
     * @brief gpio input and output init
     * gpio function multiplex registers 
     * P00~P03:MFP0, P04~P07:MFP1
     * P10~P13:MFP2, P14:MFP3, P15:EXTMFP0, P16~P17:EXTMFP1
     * P23~P26:EXTMFP3, P27/P30:EXTMFP4
     * if not assign anything, the default is gpio 
     * 
     */

    #if (GPIO_PIN_P0_MODE != P0_ALL_INPUT)
        CLEAR_BIT(P0, GPIO_PIN_P0_MODE);
        CLEAR_BIT(P0MOD, GPIO_PIN_P0_MODE);
    #endif

    #if (GPIO_PIN_P1_MODE != P1_ALL_INPUT)
        CLEAR_BIT(P1, (GPIO_PIN_P1_MODE & P1_MASK));
        CLEAR_BIT(P1MOD, (GPIO_PIN_P1_MODE & P1_MASK));
    #endif

    #if (GPIO_PIN_P1_EFR_MODE != P1_EFR_INPUT)
        CLEAR_EFR_BIT(PEXTCON0, ((GPIO_PIN_P1_EFR_MODE >> 1)|(GPIO_PIN_P1_EFR_MODE >> 5)));
    #endif

    #if (GPIO_PIN_P2_EFR_MODE != P2_EFR_INPUT)
        CLEAR_EFR_BIT(PEXTCON2, (GPIO_PIN_P2_EFR_MODE|(GPIO_PIN_P2_EFR_MODE << 4)));
    #endif

    #if (CONFIG_IC_TYPE == CHIP_SS888X)
        #if (GPIO_PIN_P2_EXT_MODE != P2_EXT_INPUT)
            CLEAR_BIT(P2, (GPIO_PIN_P2_EXT_MODE & P2_MASK));
            CLEAR_BIT(P2MOD, (GPIO_PIN_P2_EXT_MODE & P2_MASK));
        #endif
    #endif

    /** TODO: Pull mode  **/
    #if(P0_PULL_MODE & GPIO_PULLUP)
        #if (GPIO_P00_PULL == GPIO_PULLUP)
            val = GPIO_0_PULLUP;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P01_PULL == GPIO_PULLUP)
            val |= GPIO_1_PULLUP;
        #endif

        #if (GPIO_P02_PULL == GPIO_PULLUP)
            val |= GPIO_2_PULLUP;
        #endif

        #if (GPIO_P03_PULL == GPIO_PULLUP)
            val |= GPIO_3_PULLUP;
        #endif

        #if (GPIO_P04_PULL == GPIO_PULLUP)
            val |= GPIO_4_PULLUP;
        #endif

        #if (GPIO_P05_PULL == GPIO_PULLUP)
            val |= GPIO_5_PULLUP;
        #endif

        #if (GPIO_P06_PULL == GPIO_PULLUP)
            val |= GPIO_6_PULLUP;
        #endif

        #if (GPIO_P07_PULL == GPIO_PULLUP)
            val |= GPIO_7_PULLUP;
        #endif

        WRITE_REG(P0PU, val);
    #endif

    #if(P0_PULL_MODE & GPIO_PULLDOWN)
        #if (GPIO_P00_PULL == GPIO_PULLDOWN)
            val = GPIO_0_PULLDOWN;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P01_PULL == GPIO_PULLDOWN)
            val |= GPIO_1_PULLDOWN;
        #endif

        #if (GPIO_P02_PULL == GPIO_PULLDOWN)
            val |= GPIO_2_PULLDOWN;
        #endif

        #if (GPIO_P03_PULL == GPIO_PULLDOWN)
            val |= GPIO_3_PULLDOWN;
        #endif

        #if (GPIO_P04_PULL == GPIO_PULLDOWN)
            val |= GPIO_4_PULLDOWN;
        #endif

        #if (GPIO_P05_PULL == GPIO_PULLDOWN)
            val |= GPIO_5_PULLDOWN;
        #endif

        #if (GPIO_P06_PULL == GPIO_PULLDOWN)
            val |= GPIO_6_PULLDOWN;
        #endif

        #if (GPIO_P07_PULL == GPIO_PULLDOWN)
            val |= GPIO_7_PULLDOWN;
        #endif
        WRITE_REG(P0PD, val);
    #endif

    #if(P1_PULL_MODE & GPIO_PULLUP)
        #if (GPIO_P10_PULL == GPIO_PULLUP)
            val = GPIO_0_PULLUP;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P11_PULL == GPIO_PULLUP)
            val |= GPIO_1_PULLUP;
        #endif

        #if (GPIO_P12_PULL == GPIO_PULLUP)
            val |= GPIO_2_PULLUP;
        #endif

        #if (GPIO_P13_PULL == GPIO_PULLUP)
            val |= GPIO_3_PULLUP;
        #endif

        #if (GPIO_P14_PULL == GPIO_PULLUP)
            SET_BIT(PDRV0, GPIO_4_PULLDOWN << 2);
        #endif

        WRITE_REG(P1PUPD, val);
    #endif

    #if(P0_PULL_MODE & GPIO_PULLDOWN)
        #if (GPIO_P10_PULL == GPIO_PULLDOWN)
            val = GPIO_0_PULLDOWN;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P11_PULL == GPIO_PULLDOWN)
            val |= GPIO_1_PULLDOWN;
        #endif

        #if (GPIO_P12_PULL == GPIO_PULLDOWN)
            val |= GPIO_2_PULLDOWN;
        #endif

        #if (GPIO_P13_PULL == GPIO_PULLDOWN)
            val |= GPIO_3_PULLDOWN;
        #endif

        #if (GPIO_P14_PULL == GPIO_PULLDOWN)
            SET_BIT(PDRV0, GPIO_4_PULLDOWN << 1);
        #endif
        WRITE_REG(P1PUPD, val);
    #endif

    #if(P1_EFR_PULL_MODE & GPIO_PULLUP)
        #if (GPIO_P15_PULL == GPIO_PULLUP)
            val = GPIO_5_PULLUP;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P16_PULL == GPIO_PULLUP)
            val |= GPIO_6_PULLUP;
        #endif

        #if (GPIO_P17_PULL == GPIO_PULLUP)
            val |= GPIO_7_PULLUP;
        #endif

        MODIFY_EFR_REG(PEXTCON1, (val >> 1), (val >> 1));
    #endif

    #if(P1_EFR_PULL_MODE & GPIO_PULLDOWN)
        #if (GPIO_P15_PULL == GPIO_PULLDOWN)
            val = GPIO_5_PULLDOWN;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P16_PULL == GPIO_PULLDOWN)
            val |= GPIO_6_PULLDOWN;
        #endif

        #if (GPIO_P17_PULL == GPIO_PULLDOWN)
            val |= GPIO_7_PULLDOWN;
        #endif
        MODIFY_EFR_REG(PEXTCON1, (val >> 5), (val >> 5));
    #endif

    #if(P2_EFR_PULL_MODE & GPIO_PULLUP)
        #if (GPIO_P20_PULL == GPIO_PULLUP)
            val = GPIO_0_PULLUP;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P21_PULL == GPIO_PULLUP)
            val |= GPIO_1_PULLUP;
        #endif

        #if (GPIO_P22_PULL == GPIO_PULLUP)
            val |= GPIO_2_PULLUP;
        #endif
        MODIFY_EFR_REG(PEXTCON3, (val << 4), (val << 4));
    #endif

    #if(P2_EFR_PULL_MODE & GPIO_PULLDOWN)
        #if (GPIO_P20_PULL == GPIO_PULLDOWN)
            val = GPIO_0_PULLDOWN;
        #else
            val = GPIO_NOPULL;
        #endif

        #if (GPIO_P21_PULL == GPIO_PULLDOWN)
            val |= GPIO_1_PULLDOWN;
        #endif

        #if (GPIO_P22_PULL == GPIO_PULLDOWN)
            val |= GPIO_2_PULLDOWN;
        #endif

        MODIFY_EFR_REG(PEXTCON3, val, val);
    #endif

    #if (CONFIG_IC_TYPE == CHIP_SS888X)
        #if(P2_PULL_MODE & GPIO_PULLUP)
            #if (GPIO_P23_PULL == GPIO_PULLUP)
                val = GPIO_3_PULLUP;
            #else
                val = GPIO_NOPULL;
            #endif

            #if (GPIO_P24_PULL == GPIO_PULLUP)
                val |= GPIO_4_PULLUP;
            #endif

            #if (GPIO_P25_PULL == GPIO_PULLUP)
                val |= GPIO_5_PULLUP;
            #endif

            #if (GPIO_P26_PULL == GPIO_PULLUP)
                val |= GPIO_6_PULLUP;
            #endif

            SET_BIT(P2PU0, val);

            #if (GPIO_P27_PULL == GPIO_PULLUP)
                _P27_IO_PULLUP(SET);
            #endif
            #if (GPIO_P30_PULL == GPIO_PULLUP)
                _P30_IO_PULLUP(SET);
            #endif
        #endif
    #endif

    /** ADC PWM CC CMP TS UART config   **/
    /** MFP0 P00-P03 **/
    #if (GPIO_P00_CMP_MODE)
        val = 0x03;
    #elif (GPIO_P00_PWM_MODE)
        val = 0x01;
    #else
        val = 0x00;
    #endif

    #if (GPIO_P01_ADC_MODE)
        val |= (0x03 << 2);
    #elif (GPIO_P01_PWM_MODE)
        val |= (0x01 << 2);
    #elif (GPIO_P01_UART_MODE & TX0)
        val |= (0x02 << 2);
    #endif    

    #if (GPIO_P02_ADC_MODE)
        val |= (0x02 << 4);
    #elif (GPIO_P02_CC_MODE)
        val |= (0x03 << 4);
    #endif

    #if (GPIO_P03_ADC_MODE)
        val |= (0x03 << 6);
    #elif (GPIO_P03_EINT_MODE)
        val |= (0x02 << 4);
    #endif
    WRITE_REG(MFP0, val);

    /** MFP1 P04-P07 **/
    #if (GPIO_P04_CMP_MODE)
        val = 0x03;
    #elif (GPIO_P04_CC_MODE)
        val = 0x01;
    #elif (GPIO_P04_EINT_MODE)
        val = 0x02;
    #else
        val = 0x00;
    #endif

    #if (GPIO_P05_CC_MODE)
        val |= (0x01 << 2);
    #elif (GPIO_P05_SPI_MODE)
        val |= (0x03 << 2);
    #endif    

    #if (GPIO_P06_ADC_MODE)
        val |= (0x03 << 4);
    #elif (GPIO_P02_CC_MODE)
        val |= (0x01 << 4);
    #elif (GPIO_P06_UART_MODE & (TX0 | RX0 | MT0))
        val |= (0x02 << 4);
    #endif

    #if (GPIO_P07_CC_MODE)
        val |= (0x01 << 6);
    #elif (GPIO_P07_PFO_MODE)
        val |= (0x02 << 6);
    #endif

    WRITE_REG(MFP1, val);

    /** MFP2 P10-P13 **/
    #if (GPIO_P10_CC_MODE)
        val = 0x03;
    #elif (GPIO_P10_EINT_MODE)
        val = 0x02;
    #else
        val = 0x00;
    #endif

    #if (GPIO_P11_ADC_MODE)
        val |= (0x03 << 2);
    #endif    

    #if (GPIO_P12_UART_MODE & RX0)
        val |= (0x02 << 4);
    #endif

    #if (GPIO_P13_PWM_MODE)
        val |= (0x02 << 6);
    #endif

    WRITE_REG(MFP2, val);

    /** MFP3 P14 P06 P11 **/
    #if (GPIO_P11_UART_MODE & (RX0 | MT0))
        val = 0x01;
    #elif (GPIO_P11_UART_MODE & TX0)
        val = 0x02;
    #else
        val = 0x00;
    #endif

    #if (GPIO_P06_UART_MODE & (RX0 | MT0))
        val |= 0x04;
    #elif (GPIO_P06_UART_MODE & TX0)
        val |= 0x08;
    #endif

    #if (GPIO_P14_CC_MODE)
        val |= 0x20;
    #elif (GPIO_P14_EINT_MODE)
        val |= 0x30;
    #endif

    MODIFY_REG(MFP3, 0x3F, val);

    /** EXTMFP0 P10 P12 P14 P15 **/
    #if (GPIO_P12_TS_MODE)
        val = 0x01;
    #else
        val = 0x00;
    #endif 

    #if (GPIO_P10_TS_MODE)
        val |= (0x01 << 2);
    #endif

    #if (GPIO_P14_TS_MODE)
        val |= (0x01 << 4);
    #elif (GPIO_P14_SPI_MODE)
        val |= (0x03 << 4);
    #endif

    #if (GPIO_P15_TS_MODE)
        val |= (0x01 << 6);
    #elif (GPIO_P15_CC_MODE)
        val |= (0x03 << 6);
    #endif

    WRITE_EFR_REG(EXTMFP0, val);

    /** EXTMFP1 P13 P16 P17 **/
    #if (GPIO_P16_TS_MODE)
        val = (0x01);
    #elif (GPIO_P16_PWM_MODE)
        val = (0x03);
    #else
        val = 0x00;
    #endif

    #if (TS_CHANNEL_MODE)
        val |= (0x40);
    #endif

    #if (GPIO_P17_TS_MODE)
        val |= (0x01 << 2);
    #elif (GPIO_P17_UART_MODE & (RX0 | MT0))
        val |= (0x02 << 2);
    #elif (GPIO_P17_UART_MODE & TX0)
        val |= (0x03 << 2);
    #endif

    MODIFY_EFR_REG(EXTMFP1, 0xCF, val);

    /** EXTMFP2 P20 P21 P22 **/
    #if (GPIO_P20_UART_MODE & (RX1 | MT1))
        val = 0x03;
    #elif (GPIO_P20_UART_MODE & TX1)
        val = 0x02;
    #elif (GPIO_P20_SPI_MODE)
        val = 0x01;
    #else
        val = 0x00;
    #endif

    #if (GPIO_P21_UART_MODE & TX1)
        val |= (0x02 << 2);
    #elif (GPIO_P21_SPI_MODE)
        val |= (0x01 << 2);
    #endif

    #if (GPIO_P22_UART_MODE & RX1)
        val |= (0x02 << 4);
    #elif (GPIO_P22_SPI_MODE)
        val |= (0x01 << 4);
    #endif

    MODIFY_EFR_REG(EXTMFP2, 0x3F, val);

    #if (CONFIG_IC_TYPE == CHIP_SS888X)
        /** EXTMFP3 P23 P24 P25 P26 **/
        #if (GPIO_P23_CC_MODE)
            val = 0x01;
        #elif (GPIO_P23_SPI_MODE)
            val = 0x02;    
        #else
            val = 0x00;
        #endif

        #if (GPIO_P24_SPI_MODE)
            val |= (0x02 << 2);
        #endif

        #if (GPIO_P25_CC_MODE)
            val |= (0x01 << 4);
        #elif (GPIO_P25_SPI_MODE)
            val |= (0x02 << 4);
        #endif

        #if (GPIO_P26_CC_MODE)
            val |= (0x01 << 6);
        #elif (GPIO_P26_SPI_MODE)
            val |= (0x02 << 6);
        #endif

        WRITE_EFR_REG(EXTMFP3, val);

        /** EXTMFP4 P27 P30 **/
        #if (GPIO_P27_CC_MODE)
            val = 0x07;
        #elif (GPIO_P27_UART_MODE & (RX1 | MT1))
            val = 0x01;
        #elif (GPIO_P27_UART_MODE & TX1)
            val = 0x02;
        #elif (GPIO_P27_UART_MODE & (RX0 | MT0))
            val = 0x06;
        #elif (GPIO_P27_UART_MODE & TX0)
            val = 0x06;
        #else
            val = 0x00;
        #endif

        #if (GPIO_P30_CC_MODE)
            val |= 0x38;
        #elif (GPIO_P30_UART_MODE & (RX1 | MT1))
            val |= 0x10;
        #elif (GPIO_P30_UART_MODE & TX1)
            val |= 0x08;
        #elif (GPIO_P30_UART_MODE & TX0)
            val |= 0x21;
        #elif (GPIO_P30_SPI_MODE)
            val |= 0x30;
        #endif

        MODIFY_EFR_REG(EXTMFP4, 0x3F, val);

        /*** TODO: EXTMFP5 ***/

        /** EXTMFP6 P16 P26 P25 P02 P11 **/
        val = 0x03;

        #if (GPIO_P25_ADC_MODE)
            val = ((val & ~0x02) | 0x04);
        #endif

        #if (GPIO_P26_ADC_MODE)
            val = ((val & ~0x01) | 0x08);
        #endif

        #if (GPIO_P16_ADC_MODE)
            val = ((val & ~0x01) | 0x10);
        #endif

        MODIFY_EFR_REG(EXTMFP6, 0x1F, val);
    #endif

    #if (GPIO_P04_EINT_MODE || GPIO_P10_EINT_MODE)
        _EX0_INTERRUPT(SET);
    #endif

    #if (GPIO_P03_EINT_MODE || GPIO_P14_EINT_MODE)
        _EX1_INTERRUPT(SET);
    #endif
}